/**
 * This path finding algorithm lets the user tell the robot where to go.
 * 
 * *Commands*
 * Left  : Robot turns left
 * Right : Robot turns right
 * Up    : Robot speeds up
 * Down  : Robot speeds down
 * Q     : Full speed
 * A     : Full reverse
 * S     : Full stop
 * R     : Reverse previous command
 * P     : Panic command
 */

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework.Input;

namespace Autobot
{
    class UserInputAlgorithm : AlgorithmBase
    {
        // Static string to identify this class in the Algorithms class
        public static readonly new string Name = "UserInput";

        // Store the passed references to the world
        public UserInputAlgorithm(VirtualMap mapRef, Robot robotRef, Goal goalRef)
        {
            map = mapRef;
            robot = robotRef;
            goal = goalRef;
        }

        // This algorithm also has to remember the keyboard/mouse inputs were on the last GetNextCommand
        private KeyboardState lastKeyboardInput = Keyboard.GetState();
        private MouseState lastMouseInput = Mouse.GetState();

        // Define what keys do what commands
        private const Keys TurnLeft    = Keys.Left,
                           TurnRight   = Keys.Right,
                           SpeedUp     = Keys.Up,
                           SpeedDown   = Keys.Down,
                           FullSpeed   = Keys.Q,
                           FullReverse = Keys.A,
                           FullStop    = Keys.S,
                           Reverse     = Keys.R,
                           Panic       = Keys.P;

        // GetNextCommand: Return what the user is inputting
        public override AlgorithmResult GetNextResult()
        {
            // Get the current user input
            KeyboardState keyboardInput = Keyboard.GetState();
            MouseState mouseInput = Mouse.GetState();

            // Prepare what to send to the robot
            AlgorithmResult result = new AlgorithmResult();

            // When the user holds down left, turn left
            if (keyboardInput.IsKeyDown(TurnLeft) && lastKeyboardInput.IsKeyUp(TurnLeft) && keyboardInput.IsKeyUp(TurnRight))
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.MIN, 0));
            // When the user stops holding down left, start going straight
            else if(keyboardInput.IsKeyUp(TurnLeft) && lastKeyboardInput.IsKeyDown(TurnLeft) && keyboardInput.IsKeyUp(TurnRight))
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));

            // When the user holds right, start turning right
            else if (keyboardInput.IsKeyDown(TurnRight) && lastKeyboardInput.IsKeyUp(TurnRight) && keyboardInput.IsKeyUp(TurnLeft))
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.MAX, 0));
            // When the user releases right, stop turning right
            else if (keyboardInput.IsKeyUp(TurnRight) && lastKeyboardInput.IsKeyDown(TurnRight) && keyboardInput.IsKeyUp(TurnLeft))
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));

            // Speed up
            else if (keyboardInput.IsKeyDown(SpeedUp) && lastKeyboardInput.IsKeyUp(SpeedUp))
                result.AddCommand(CreateIncSpeedCommand(1, 0));
            // Speed down
            else if (keyboardInput.IsKeyDown(SpeedDown) && lastKeyboardInput.IsKeyUp(SpeedDown))
                result.AddCommand(CreateDecSpeedCommand(1, 0));
            // Set speed to maximum!
            else if (keyboardInput.IsKeyDown(FullSpeed) && lastKeyboardInput.IsKeyUp(FullSpeed))
                result.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MAX, 0));
            // Set speed to maximum reverse
            else if (keyboardInput.IsKeyDown(FullReverse) && lastKeyboardInput.IsKeyUp(FullReverse))
                result.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MIN, 0));
            // Full stop
            else if (keyboardInput.IsKeyDown(FullStop) && lastKeyboardInput.IsKeyUp(FullStop))
                result.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.STOP, 0));

            // Robot does the reverse of its last command (there must have been a previous command) 
            // and removes it from the previous command stack
            //else if (keyboardInput.IsKeyDown(Reverse) && lastKeyboardInput.IsKeyUp(Reverse) && previousResults.Count > 0)
                //Command = this.ReverseCommand(previousResults.Pop().GetCommand(0));

            // Robot panics
            else if (keyboardInput.IsKeyDown(Panic) && lastKeyboardInput.IsKeyUp(Panic))
                result.AddCommand(CreatePanicCommand(0));

            // Store the current input states as the last inputs
            lastKeyboardInput = keyboardInput;
            lastMouseInput = mouseInput;

            // Remember the command if the remember bool is true and the command isn't NoCommand
            //if (remember == true && Command.Action != (int)AlgorithmAction.NONE)
                //previousResults.Push(NewResult);

            if (result.GetNumCommands() == 0)
                result.AddCommand(CreateNoCommand());

            return result;
        }
    }
}
